﻿
#include "stdafx.h"
#include "ephcheck.h"
#include "Gpsstr.h"
#include<cmath>
#include<string.h>
const BYTE GPSPatID[] = { 5, 6, 6, 6, 10, 8, 6, 7, 7, 7, 7, 7, 9 };
const double BD2_ACCURACY[] = { 2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0, 3072.0, 6144.0, 9.999999999999e99 };
const double GPS_ACCURACY[] = { 0.01, 0.02, 0.03, 0.04, 0.06, 0.08, 0.11, 0.15, 0.21, 0.30, 0.43, 0.60, 0.85, 1.20, 1.70,
2.0, 2.8, 4.0, 5.7, 8.0, 11.3, 16.0, 32.0, 64.0, 128.0, 256.0, 512.0, 1024.0, 2048.0, 4096.0, 8192 };
const int WordSize = 10;
double M_PI = 3.1415926535898;
struct GPSCal_sfEph GPSL_EphCal;
struct BD2Cal_sfEph BD2_EphCal;
extern struct StructObsBuff databuff_obsbuffdata[BD2PrnNum];
extern struct StructGpsEphmeris_LNav    GpsEphmeris_LNav[GpsPrnNum];
extern struct StructBD2Ephmeris     BD2Ephmeris_B1[BD2PrnNum];


struct StructSOW check_timer_data(struct StructSOW A)
{
	struct StructSOW result;
	result = A;
	while (result.sub_msec < 0)
	{
		result.msec--;
		result.sub_msec += SUB_MSEC_MAX;
	}
	while (result.sub_msec >= SUB_MSEC_MAX)
	{
		result.msec++;
		result.sub_msec -= SUB_MSEC_MAX;
	}
	return result;
}

struct StructSOW substract_timer(struct StructSOW A, struct StructSOW B)
{
	struct StructSOW result;
	result.msec = A.msec - B.msec;
	result.sub_msec = A.sub_msec - B.sub_msec;
	result = check_timer_data(result);
	return result;
}

double substract_timer_double(struct StructSOW A, struct StructSOW B)
{
	double result;
	struct StructSOW tmp;
	tmp = substract_timer(A, B);
	result = tmp.msec / 1000.0 + tmp.sub_msec / 1e12;
	//  sprintf(test_str, "time sub ms%d, %d, \r\n", tmp.msec, tmp.sub_msec);
	//  USR_Ser_WrStr (test_str, BSP_SER_COMM_UART_03);
	return result;
}

struct StructSOW add_timer(struct StructSOW A, struct StructSOW B)
{
	struct StructSOW result;
	result.msec = A.msec + B.msec;
	result.sub_msec = A.sub_msec + B.sub_msec;
	result = check_timer_data(result);
	return result;
}

struct StructSOW format_timer(double A)
{
	struct StructSOW result;
	result.msec = (long)(A * 1000);
	result.sub_msec = (long)((A - result.msec / 1e3) * 1e12);
	result = check_timer_data(result);
	return result;
}

struct StructSOW format_timer_msps(long ms, long ps)
{
	struct StructSOW result;
	result.msec = ms;
	result.sub_msec = ps;
	return result;
}
double  checktime(double time)
{
	double result = time;
	if (time > HalfWeek)
	{
		result = time - SecWeek;
	}
	else if (time < -1 * HalfWeek)
	{
		result = time + SecWeek;
	}
	else
	{
		result = time;
	}
	return result;
}

void MatrixMult(double* p_result, double* p_Matrix1, double* p_Matrix2, int R_M1, int C_M1, int R_M2, int C_M2)     // CM1 == RM2
{
	int i1, k, j2;
	memset(p_result, 0, sizeof(double)*R_M1 * C_M2);
	for (i1 = 0; i1 < R_M1; i1++)
	{
		for (j2 = 0; j2 < C_M2; j2++)
		{
			for (k = 0; k < C_M1; k++)
			{
				p_result[i1 * C_M2 + j2] += p_Matrix1[i1 * C_M1 + k] * p_Matrix2[k * C_M2 + j2];
			}
		}
	}
}


void MatrixAdd(double* p_result, double* p_Matrix1, double* p_Matrix2, int R, int C)
{
	int i, j;
	for (i = 0; i < R; i++)
	{
		for (j = 0; j < C; j++)
		{
			p_result[i * C + j] = p_Matrix1[i * C + j] + p_Matrix2[i * C + j];
		}
	}
}

bool Gpsparitycheckword(Uint32 word, Uint8 D29star, Uint8 D30star)
{
	bool result;
	bool result25, result26, result27, result28, result29, result30;
	int idx = 0;
	int tmpword = 0;
	int tmptmp = 0;
	Uint8 buffer[6];
	//计算d1至d24的数据。
	//by longgo modify 
	// MSB位于word高位
	/*
	tmpword = 0;
	if(D30star == 1)
	{
	for(idx = 0; idx < WordBitSize; idx++)
	{
	tmptmp = tmpword << 1;
	tmpword = tmptmp + !buff[idx];
	}
	}
	else
	{
	for(idx = 0; idx < WordBitSize; idx++)
	{
	tmptmp = tmpword << 1;
	tmpword = tmptmp + buff[idx];
	}
	}
	*word_out = tmpword;



	// MSB位于word高位
	tmpword = 0;
	if(D30star == 1)
	{
	for(idx = 0; idx < WordBitSize-6; idx++)
	{
	tmptmp = tmpword << 1;
	tmpword = tmptmp + !buff[idx];
	}
	}
	else
	{
	for(idx = 0; idx < WordBitSize-6; idx++)
	{
	tmptmp = tmpword << 1;
	tmpword = tmptmp + buff[idx];
	}
	}
	*word = tmpword;
	*/

	//将D29star，D30star设置于tmpword的31，32比特位。用于模2和计算D25至D30.
	if (D30star == 1)
	{
		tmpword = word^Data30Xor;
	}
	else
	{
		tmpword = word;
	}


	for (int i = 0; i<6; i++)
	{
		buffer[i] = ((tmpword&(0x01 << i)) >> i);//0 D30 

	}

	tmpword = tmpword >> 6;

	tmpword = tmpword + (D29star << 30);
	tmpword = tmpword + (D30star << 31);

	result25 = GpsXors(tmpword&D25bitpos) == buffer[5];
	result26 = GpsXors(tmpword&D26bitpos) == buffer[4];
	result27 = GpsXors(tmpword&D27bitpos) == buffer[3];
	result28 = GpsXors(tmpword&D28bitpos) == buffer[2];
	result29 = GpsXors(tmpword&D29bitpos) == buffer[1];
	result30 = GpsXors(tmpword&D30bitpos) == buffer[0];


	//result = (GpsXors(tmpword&D25bitpos) == buffer[24])&(GpsXors(tmpword&D26bitpos) == buffer[25]) & (GpsXors(tmpword&D27bitpos) == buffer[26]) & (GpsXors(tmpword&D28bitpos) == buffer[27]) & (GpsXors(tmpword&D29bitpos) == buffer[28]) & (GpsXors(tmpword&D30bitpos) == buffer[29]);
	result = result25&result26&result27&result28&result29&result30;
	return result;
}

int  GpsXors(Uint32  word)
{
	Uint8 idx = 0;
	Uint8 tmpsum = 0;
	Uint8 tmp = 0;
	for (idx = 0; idx < WordBitSize + 2; idx++)
	{
		tmp = word % 2;
		tmpsum = tmpsum + tmp;
		word = word >> 1;
	}
	return tmpsum % 2;


}


long    Gps_LNAV_Eph(unsigned long* word, struct StructGpsEphmeris_LNav* p_TmpEphmeris, struct StructGpsEphmeris_LNav* p_Ephmeris)
{
	long idx;
	bool EphChangeFlag = false;
	//依据IODE条件来更新星历信息。
	if ((p_TmpEphmeris->IODCL8 != p_TmpEphmeris->IODE[0]) || (p_TmpEphmeris->IODCL8 != p_TmpEphmeris->IODE[1]) || (p_TmpEphmeris->IODE[1] != p_TmpEphmeris->IODE[0]))
	{
		EphChangeFlag = true;
	}
	else
	{
		EphChangeFlag = false;
	}
	p_TmpEphmeris->HOWtime = (long)((word[1] & 0x00FFFF80) >> 7);
	switch (p_TmpEphmeris->idofsubframe)
	{
	case 1:
		if ((DWORD)((word[7] & 0x0000FFFF) << 4) != p_TmpEphmeris->Toc) {
			EphChangeFlag = true;
			p_TmpEphmeris->haveSubframe[0] = false;
			p_TmpEphmeris->haveSubframe[1] = false;
			p_TmpEphmeris->haveSubframe[2] = false;
		}
		if ((!p_TmpEphmeris->haveSubframe[0]) || EphChangeFlag)
		{
			p_TmpEphmeris->tlm_message[0] = (WORD)(word[0] & 0x00ffff);
			p_TmpEphmeris->ASalert[0] = (BYTE)((word[1] & 0x00000060) >> 5);
			p_TmpEphmeris->WeekNum = (WORD)(((word[2] & 0x00FFC000) >> 14) + 2048); //
			p_TmpEphmeris->CodeOnL2 = ((char)((word[2] & 0x00003000) >> 12));
			p_TmpEphmeris->URA = ((char)((word[2] & 0x00000F00) >> 5)) >> 3;
			p_TmpEphmeris->SVhealth = (BYTE)((word[2] & 0x000000FC) >> 2);
			p_TmpEphmeris->IODCL8 = (WORD)(((word[7] & 0x00FF0000) >> 16));
			p_TmpEphmeris->IODC = (WORD)(((word[2] & 0x00000003) << 8) | ((word[7] & 0x00FF0000) >> 16));
			p_TmpEphmeris->L2Pdata = (BYTE)((word[3] & 0x00800000) >> 23);
			p_TmpEphmeris->Tgd = ldexp((double)((char)(word[6] & 0x000000FF)), -31);
			p_TmpEphmeris->Toc = (DWORD)((word[7] & 0x0000FFFF) << 4);
			p_TmpEphmeris->af2 = ldexp((double)((char)((word[8] & 0x00FF0000) >> 16)), -55);
			p_TmpEphmeris->af1 = ldexp((double)(short)((word[8] & 0x0000FFFF)), -43);
			p_TmpEphmeris->af0 = ldexp((double)((long)((word[9] & 0x00FFFFFC) << 8)), -(31 + 10));
			p_TmpEphmeris->haveSubframe[0] = true;
			// convert the accuracy flag to a value...
			if (p_TmpEphmeris->URA > 15 || p_TmpEphmeris->URA < 0)
			{
				p_TmpEphmeris->accuracy = GPS_ACCURACY[30];
			}
			else
			{
				p_TmpEphmeris->accuracy = GPS_ACCURACY[p_TmpEphmeris->URA + 15];
			}
		}
		else
		{
			p_TmpEphmeris->tlm_message[0] = (WORD)(word[0] & 0x00ffff);
			p_TmpEphmeris->HOWtime = (long)((word[1] & 0x00FFFF80) >> 7);
			p_TmpEphmeris->ASalert[0] = (BYTE)((word[1] & 0x00000060) >> 5);
		}
		break;
	case 2:
		if ((!p_TmpEphmeris->haveSubframe[1]) || EphChangeFlag)
		{
			p_TmpEphmeris->tlm_message[1] = (WORD)(word[0] & 0x00ffff);
			p_TmpEphmeris->ASalert[1] = (BYTE)((word[1] & 0x00000060) >> 5);
			p_TmpEphmeris->IODE[0] = (WORD)((word[2] & 0x00FF0000) >> 16);
			p_TmpEphmeris->Crs = ldexp((double)((short)(word[2] & 0x0000FFFF)), -5);
			p_TmpEphmeris->M0 = M_PI * ldexp((double)((long)(((word[3] & 0x000000FF) << 24) | word[4])), -31);
			p_TmpEphmeris->Cuc = ldexp((double)((short)((word[5] & 0x00FFFF00) >> 8)), -29);
			p_TmpEphmeris->ecc = ldexp((double)((DWORD)(((word[5] & 0x000000FF) << 24) | word[6])), -33);
			p_TmpEphmeris->Cus = ldexp((double)((short)((word[7] & 0x00FFFF00) >> 8)), -29);
			p_TmpEphmeris->Asqrt = ldexp((double)((DWORD)(((word[7] & 0x000000FF) << 24) | word[8])), -19);
			p_TmpEphmeris->Toe = ldexp((double)((WORD)((word[9] & 0x00FFFF00) >> 8)), 4);
			p_TmpEphmeris->FitInt = (BYTE)((word[9] & 0x00000080) >> 7);
			p_TmpEphmeris->fit = p_TmpEphmeris->FitInt?0.0:4.0; /* 0:4hr,1:>4hr */
			p_TmpEphmeris->dn = M_PI * ldexp((double)((short)((word[3] & 0x00FFFF00) >> 8)), -43);
			p_TmpEphmeris->haveSubframe[1] = true;
		}
		else
		{
			p_TmpEphmeris->tlm_message[1] = (WORD)(word[0] & 0x00ffff);
			p_TmpEphmeris->HOWtime = (long)((word[1] & 0x00FFFF80) >> 7);
			p_TmpEphmeris->ASalert[1] = (BYTE)((word[1] & 0x00000060) >> 5);
		}
		break;
	case 3:
		if ((!p_TmpEphmeris->haveSubframe[2]) || EphChangeFlag)
		{
			p_TmpEphmeris->tlm_message[2] = (WORD)(word[0] & 0x00ffff);
			p_TmpEphmeris->ASalert[2] = (BYTE)((word[1] & 0x00000060) >> 5);
			p_TmpEphmeris->Cic = ldexp((double)((short)((word[2] & 0x00FFFF00) >> 8)), -29);
			p_TmpEphmeris->OMEGA0 = M_PI * ldexp((double)((long)(((word[2] & 0x000000FF) << 24) | word[3])), -31);
			p_TmpEphmeris->Cis = ldexp((double)((short)((word[4] & 0x00FFFF00) >> 8)), -29);
			p_TmpEphmeris->i0 = M_PI * ldexp((double)((long)(((word[4] & 0x000000FF) << 24) | word[5])), -31);
			p_TmpEphmeris->Crc = ldexp((double)((short)((word[6] & 0x00FFFF00) >> 8)), -5);
			p_TmpEphmeris->w = M_PI * ldexp((double)((long)(((word[6] & 0x000000FF) << 24) | word[7])), -31);
			p_TmpEphmeris->OMEGAdot = M_PI * ldexp((double)((long)((word[8] & 0x00FFFFFF) << 8)), -(43 + 8));
			p_TmpEphmeris->IODE[1] = (WORD)((word[9] & 0x00FF0000) >> 16);
			p_TmpEphmeris->idot = M_PI * ldexp((double)((short)(word[9] & 0x0000FFFC)), -(43 + 2));
			p_TmpEphmeris->haveSubframe[2] = true;
		}
		else
		{
			p_TmpEphmeris->tlm_message[2] = (WORD)(word[0] & 0x00ffff);
			p_TmpEphmeris->HOWtime = (long)((word[1] & 0x00FFFF80) >> 7);
			p_TmpEphmeris->ASalert[2] = (BYTE)((word[1] & 0x00000060) >> 5);
		}
		break;
	default:
		//getchar();
		break;
	}
	//存储电文。
	for (idx = 0; idx < WordSize; idx++)
	{
		p_TmpEphmeris->word[(p_TmpEphmeris->idofsubframe - 1)][idx] = word[idx];
	}
	if (p_TmpEphmeris->haveSubframe[0] && p_TmpEphmeris->haveSubframe[1] && p_TmpEphmeris->haveSubframe[2])
	{
		p_TmpEphmeris->beSVpos = true;    //卫星能解算出位置的条件为已经接收到了前三个子帧。
	}
	p_Ephmeris->HOWtime = p_TmpEphmeris->HOWtime;
	if (p_TmpEphmeris->idofsubframe == 3 && p_TmpEphmeris->beSVpos == true)        // 星历更新完成
	{
		memcpy(p_Ephmeris, p_TmpEphmeris, sizeof(struct StructGpsEphmeris_LNav));
	}
	return 1;
}


long    BD2_Eph_D1(unsigned long* word, struct StructBD2Ephmeris* p_TmpEphmeris, struct StructBD2Ephmeris* p_Ephmeris)
{
	long idx;
	p_TmpEphmeris->SOW = (long)(((word[0] & 0x00000FF0) << 8) | ((word[1] & 0x3FFC0000) >> 18));
	switch (p_TmpEphmeris->idofsubframe)
	{
	case 1:
		if (((DWORD)((word[2] & 0x0001FF00) | ((word[3] & 0x3FC00000) >> 22))) << 3 != p_TmpEphmeris->Toc) {
			p_TmpEphmeris->haveSubframe[0] = false;
			p_TmpEphmeris->haveSubframe[1] = false;
			p_TmpEphmeris->haveSubframe[2] = false;
		}
		p_TmpEphmeris->WeekNum = (WORD)((word[2] & 0x3FFE0000) >> 17);
		p_TmpEphmeris->URAI = (BYTE)((word[1] & 0x00000F00) >> 8);
		p_TmpEphmeris->SatH1 = (BYTE)((word[1] & 0x00020000) >> 17);
		p_TmpEphmeris->AODC = (WORD)((word[1] & 0x0001F000) >> 12);
		p_TmpEphmeris->Tgd1 = ((double)(((short)((word[3] & 0x003FF000) >> 6)) >> 6)) * 1e-10;
		p_TmpEphmeris->Tgd2 = ((double)(((short)(((word[3] & 0x00000F00) << 4) | ((word[4] & 0x3F000000) >> 14))) >> 6)) * 1e-10;
		p_TmpEphmeris->Toc = ((DWORD)((word[2] & 0x0001FF00) | ((word[3] & 0x3FC00000) >> 22))) << 3;
		p_TmpEphmeris->alpha[0] = ldexp((double)((signed char)((word[4] & 0x00ff0000) >> 16)), -30);
		p_TmpEphmeris->alpha[1] = ldexp((double)((signed char)((word[4] & 0x0000ff00) >> 8)), -27);
		p_TmpEphmeris->alpha[2] = ldexp((double)((signed char)((word[5] & 0x3FC00000) >> 22)), -24);
		p_TmpEphmeris->alpha[3] = ldexp((double)((signed char)((word[5] & 0x003FC000) >> 14)), -24);
		p_TmpEphmeris->beta[0] = ldexp((double)((signed char)(((word[5] & 0x00003F00) >> 6) | ((word[6] & 0x30000000) >> 28))), 11);
		p_TmpEphmeris->beta[1] = ldexp((double)((signed char)((word[6] & 0x0FF00000) >> 20)), 14);
		p_TmpEphmeris->beta[2] = ldexp((double)((signed char)((word[6] & 0x000FF000) >> 12)), 16);
		p_TmpEphmeris->beta[3] = ldexp((double)((signed char)(((word[6] & 0x00000F00) >> 4) | ((word[7] & 0x3C000000) >> 26))), 16);
		p_TmpEphmeris->a2 = ldexp((double)((short)((word[7] & 0x01FF8000) >> 10)), -(66 + 5));
		p_TmpEphmeris->a1 = ldexp((double)((long)(((word[8] & 0x00001F00) << 19) | ((word[9] & 0x3FFFE000) >> 3))), -(50 + 10));
		p_TmpEphmeris->a0 = ldexp((double)((long)(((word[7] & 0x00007F00) << 17) | ((word[8] & 0x3FFFE000) >> 5))), -(33 + 8));
		p_TmpEphmeris->AODE = (WORD)((word[9] & 0x00001F00) >> 8);
		p_TmpEphmeris->haveSubframe[0] = true;
		// convert the accuracy flag to a value...
		p_TmpEphmeris->accuracy = BD2_ACCURACY[p_TmpEphmeris->URAI];
		break;
	case 2:
		p_TmpEphmeris->dn = M_PI * ldexp((double)((short)(((word[1] & 0x0003FF00) >> 2) | ((word[2] & 0x3F000000) >> 24))), -43);
		p_TmpEphmeris->Cuc = ldexp((double)((long)(((word[2] & 0x00FFFF00) << 8) | ((word[3] & 0x30000000) >> 14))), -(31 + 14));
		p_TmpEphmeris->M0 = M_PI * ldexp((double)((long)(((word[3] & 0x0FFFFF00) << 4) | ((word[4] & 0x3FFC0000) >> 18))), -31);
		p_TmpEphmeris->ecc = ldexp((double)((DWORD)((long)(((word[4] & 0x0003FF00) << 14) | ((word[5] & 0x3FFFFF00) >> 8)))), -33);
		p_TmpEphmeris->Cus = ldexp((double)((long)((word[6] & 0x3FFFF000) << 2)), -(31 + 14));
		p_TmpEphmeris->Crc = ldexp((double)((long)(((word[6] & 0x00000F00) << 20) | ((word[7] & 0x3FFF0000) >> 2))), -(6 + 14));
		p_TmpEphmeris->Crs = ldexp((double)((long)(((word[7] & 0x0000FF00) << 16) | ((word[8] & 0x3FF00000) >> 6))), -(6 + 14));
		p_TmpEphmeris->Asqrt = ldexp((double)((DWORD)(((word[8] & 0x000FFF00) << 12) | ((word[9] & 0x3FFFFC00) >> 10))), -19);
		if (p_TmpEphmeris->haveSubframe[1] == false)
		{
			p_TmpEphmeris->Toe = ((DWORD)(word[9] & 0x00000300)) << (3 + 7);    // highest 2 bit
		}
		else
		{
			p_TmpEphmeris->Toe = (p_TmpEphmeris->Toe & 0x0003FFF8) | (((DWORD)(word[9] & 0x00000300)) << (3 + 7));    // highest 2 bit
		}
		p_TmpEphmeris->haveSubframe[1] = true;
		break;
	case 3:
		p_TmpEphmeris->Toe = (p_TmpEphmeris->Toe & 0x000C0000) | ((DWORD)((word[1] & 0x0003FF00) | ((word[2] & 0x3E000000) >> 22)));       // lower 15 bit
		p_TmpEphmeris->i0 = M_PI * ldexp((double)((long)(((word[2] & 0x01FFFF00) << 7) | ((word[3] & 0x3FFF8000) >> 15))), -31);
		p_TmpEphmeris->Cic = ldexp((double)((long)(((word[3] & 0x00007F00) << 17) | ((word[4] & 0x3FF80000) >> 5))), -(31 + 14));
		p_TmpEphmeris->OMEGAdot = M_PI * ldexp((double)((long)(((word[4] & 0x0007FF00) << 13) | ((word[5] & 0x3FFE0000) >> 9))), -(43 + 8));
		p_TmpEphmeris->Cis = ldexp((double)((long)(((word[5] & 0x0001FF00) << 15) | ((word[6] & 0x3FE00000) >> 7))), -(31 + 14));
		p_TmpEphmeris->idot = M_PI * ldexp((double)((short)(((word[6] & 0x001FFF00) >> 5) | ((word[7] & 0x20000000) >> 27))), -(43 + 2));
		p_TmpEphmeris->OMEGA0 = M_PI * ldexp((double)((long)(((word[7] & 0x1FFFFF00) << 3) | ((word[8] & 0x3FF80000) >> 19))), -31);
		p_TmpEphmeris->w = M_PI * ldexp((double)((long)(((word[8] & 0x0007FF00) << 13) | ((word[9] & 0x3FFFFE00) >> 9))), -31);
		p_TmpEphmeris->haveSubframe[2] = true;
		break;
	default:
		//getchar();
		break;
	}
	//存储电文。
	for (idx = 0; idx < WordSize; idx++)
	{
		p_TmpEphmeris->word[(p_TmpEphmeris->idofsubframe - 1)][idx] = word[idx];
	}
	if (p_TmpEphmeris->haveSubframe[0] && p_TmpEphmeris->haveSubframe[1] && p_TmpEphmeris->haveSubframe[2])
	{
		p_TmpEphmeris->beSVpos = true;    //卫星能解算出位置的条件为已经接收到了前三个子帧。
	}
	p_Ephmeris->SOW = p_TmpEphmeris->SOW;
	if (p_TmpEphmeris->idofsubframe == 3 && p_TmpEphmeris->beSVpos == true)        // 星历更新完成
	{
		memcpy(p_Ephmeris, p_TmpEphmeris, sizeof(struct StructBD2Ephmeris));
	}
	return 1;
}

long    BD2_Eph_D2(unsigned long* word, struct StructBD2Ephmeris* p_TmpEphmeris, struct StructBD2Ephmeris* p_Ephmeris)
{
	long idx;
	long id_sf;
	p_TmpEphmeris->idofpage_subframe1 = (BYTE)((word[1] & 0x0003C000) >> 14);
	p_TmpEphmeris->SOW = (long)(((word[0] & 0x00000FF0) << 8) | ((word[1] & 0x3FFC0000) >> 18));
	switch (p_TmpEphmeris->idofpage_subframe1)
	{
	case 1:
		if (((DWORD)(((word[2] & 0x00001F00) << 4) | ((word[3] & 0x3FFC0000) >> 18))) << 3 != p_TmpEphmeris->Toc) {
			for (int i = 0; i < 10; i++) p_TmpEphmeris->haveallpage[i] = false;
		}
		p_TmpEphmeris->SatH1 = (BYTE)((word[1] & 0x00002000) >> 13);
		p_TmpEphmeris->AODC = (WORD)((word[1] & 0x00001F00) >> 8);
		p_TmpEphmeris->URAI = (BYTE)((word[2] & 0x3C000000) >> 26);
		p_TmpEphmeris->WeekNum = (WORD)((word[2] & 0x03FFE000) >> 13);
		p_TmpEphmeris->Toc = ((DWORD)(((word[2] & 0x00001F00) << 4) | ((word[3] & 0x3FFC0000) >> 18))) << 3;
		p_TmpEphmeris->Tgd1 = ((double)(((short)((word[3] & 0x0003FF00) >> 2)) >> 6)) * 1e-10;
		p_TmpEphmeris->Tgd2 = ((double)(((short)((word[4] & 0x3FF00000) >> 14)) >> 6)) * 1e-10;
		// convert the accuracy flag to a value...
		p_TmpEphmeris->accuracy = BD2_ACCURACY[p_TmpEphmeris->URAI];
		p_TmpEphmeris->haveallpage[0] = true;
		break;
	case 2:
		p_TmpEphmeris->haveallpage[1] = true;
		break;
	case 3:
		p_TmpEphmeris->a0 = ldexp((double)((long)(((word[3] & 0x000FFF00) << 12) | ((word[4] & 0x3FFC0000) >> 10))), -(33 + 8));
		p_TmpEphmeris->a1h = (DWORD)((word[4] & 0x0003C000) << 14);
		p_TmpEphmeris->haveallpage[2] = true;
		break;
	case 4:
		p_TmpEphmeris->a1 = ldexp((double)((long)(p_TmpEphmeris->a1h | ((word[1] & 0x00003F00) << 14) | ((word[2] & 0x3FFC0000) >> 8))), -(50 + 10));
		p_TmpEphmeris->a2 = ldexp((double)((short)(((word[2] & 0x0003FF00) >> 2) | ((word[3] & 0x20000000) >> 24))), -(66 + 5));
		p_TmpEphmeris->AODE = (WORD)(((word[3] & 0x1F000000) >> 24));
		p_TmpEphmeris->dn = M_PI * ldexp((double)((short)((word[3] & 0x00FFFF00) >> 8)), -43);
		p_TmpEphmeris->Cuch = (DWORD)((word[4] & 0x3FFF0000) << 2);
		p_TmpEphmeris->haveallpage[3] = true;
		break;
	case 5:
		p_TmpEphmeris->Cuc = ldexp((double)((long)(p_TmpEphmeris->Cuch | ((word[1] & 0x00003C00) << 4))), -(31 + 14));
		p_TmpEphmeris->M0 = M_PI * ldexp((double)((long)(((word[1] & 0x00000300) << 22) | (word[2] & 0x3FFFFF00) | ((word[3] & 0x3FC00000) >> 22))), -31);
		p_TmpEphmeris->Cus = ldexp((double)((long)(((word[3] & 0x003FFF00) << 10) | ((word[4] & 0x3C000000) >> 12))), -(31 + 14));
		p_TmpEphmeris->ecch = (DWORD)((word[4] & 0x03FF0000) << 6);
		p_TmpEphmeris->haveallpage[4] = true;
		break;
	case 6:
		p_TmpEphmeris->ecc = ldexp((double)((DWORD)(p_TmpEphmeris->ecch | ((word[1] & 0x00003F00) << 8) | ((word[2] & 0x3FFFC000) >> 14))), -33);
		p_TmpEphmeris->Asqrt = ldexp((double)((DWORD)(((word[2] & 0x00003F00) << 18) | ((word[3] & 0x3FFFFF00) >> 4) | ((word[4] & 0x3C000000) >> 26))), -19);
		p_TmpEphmeris->Cich = (DWORD)((word[4] & 0x03FF0000) << 6);
		p_TmpEphmeris->haveallpage[5] = true;
		break;
	case 7:
		p_TmpEphmeris->Cic = ldexp((double)((long)(p_TmpEphmeris->Cich | ((word[1] & 0x00003F00) << 8) | ((word[2] & 0x30000000) >> 14))), -(31 + 14));
		p_TmpEphmeris->Cis = ldexp((double)((long)((word[2] & 0x0FFFFC00) << 4)), -(31 + 14));
		p_TmpEphmeris->Toe = ((DWORD)(((word[2] & 0x00000300) << 7) | ((word[3] & 0x3FFF8000) >> 15))) << 3;
		p_TmpEphmeris->i0h = (DWORD)(((word[3] & 0x00007F00) << 17) | ((word[4] & 0x3FFF0000) >> 5));
		p_TmpEphmeris->haveallpage[6] = true;
		break;
	case 8:
		p_TmpEphmeris->i0 = M_PI * ldexp((double)((long)(p_TmpEphmeris->i0h | ((word[1] & 0x00003F00) >> 3) | ((word[2] & 0x3E000000) >> 25))), -31);
		p_TmpEphmeris->Crc = ldexp((double)((long)(((word[2] & 0x01FFFF00) << 7) | ((word[3] & 0x20000000) >> 15))), -(6 + 14));
		p_TmpEphmeris->Crs = ldexp((double)((long)((word[3] & 0x1FFFF800) << 3)), -(6 + 14));
		p_TmpEphmeris->OMEGAdoth = (DWORD)(((word[3] & 0x00000700) << 21) | ((word[4] & 0x3FFFC000) >> 1));
		p_TmpEphmeris->haveallpage[7] = true;
		break;
	case 9:
		p_TmpEphmeris->OMEGAdot = M_PI * ldexp((double)((long)(p_TmpEphmeris->OMEGAdoth | ((word[1] & 0x00003E00) >> 1))), -(43 + 8));
		p_TmpEphmeris->OMEGA0 = M_PI * ldexp((double)((long)(((word[1] & 0x00000100) << 23) | ((word[2] & 0x3FFFFF00) << 1) | ((word[3] & 0x3FE00000) >> 21))), -31);
		p_TmpEphmeris->wh = (DWORD)(((word[3] & 0x001FFF00) << 11) | ((word[4] & 0x3FFF0000) >> 11));
		p_TmpEphmeris->haveallpage[8] = true;
		break;
	case 10:
		p_TmpEphmeris->w = M_PI * ldexp((double)((long)(p_TmpEphmeris->wh | ((word[1] & 0x00003E00) >> 9))), -31);
		p_TmpEphmeris->idot = M_PI * ldexp((double)((short)(((word[1] & 0x00000100) << 7) | ((word[2] & 0x3FFE0000) >> 15))), -(43 + 2));
		p_TmpEphmeris->haveallpage[9] = true;
		break;
	default:
		//getchar();
		break;
	}
	//存储电文。
	id_sf = (p_TmpEphmeris->idofpage_subframe1 - 1) / 2;
	if (p_TmpEphmeris->idofpage_subframe1 & 0x01)
	{
		for (idx = 0; idx < 5; idx++)
		{
			p_TmpEphmeris->word[id_sf][idx] = word[idx] << 2;
		}
	}
	else
	{
		for (idx = 0; idx < 5; idx++)
		{
			p_TmpEphmeris->word[id_sf][idx + 5] = word[idx] << 2;
		}
	}
	for (idx = 0; idx < 10; idx++)
	{
		if (p_TmpEphmeris->haveallpage[idx] != true)
		{
			p_Ephmeris->SOW = p_TmpEphmeris->SOW;
			p_TmpEphmeris->beSVpos = false;  
			return 1;
		}
	}
	p_TmpEphmeris->beSVpos = true;        //卫星能解算出位置的条件为已经接收到了前10个子帧。
	p_Ephmeris->SOW = p_TmpEphmeris->SOW;
	if (p_TmpEphmeris->idofpage_subframe1 == 10)     // 星历更新完成
	{
		//          if(p_TmpEphmeris->AODE==(p_TmpEphmeris->AODC&0x1F))
		//          {
		memcpy(p_Ephmeris, p_TmpEphmeris, sizeof(struct StructBD2Ephmeris));
		p_TmpEphmeris->idofpage_subframe1 = 0;
		//        p_TmpEphmeris->haveallpage = false;///待修改
		//        memset(p_TmpEphmeris->haveallpage, 0, sizeof(bool)*10);
	//          }
	//          else
	//          {
	////                USR_Ser_WrStr("GEO nmsg error\n\r", BSP_SER_COMM_UART_03);
	//                snprintf(test_str, 512, "GEO nmsg erro AODE:%d AODC%d\r\n", (int)(p_TmpEphmeris->AODE), (int)(p_TmpEphmeris->AODC));
	//                USR_Ser_WrStr (test_str, BSP_SER_COMM_UART_03);
	//                memset(p_Ephmeris, 0, sizeof(struct StructBD2Ephmeris));
	//                memset(p_TmpEphmeris, 0, sizeof(struct StructBD2Ephmeris));
	//                p_TmpEphmeris->beSVpos = false;
	//          }
	}
	return 1;
}
// 历书数据解析（不用）
long GpsLNAVAlmDisassemble(unsigned long *word , struct StructGpsAlmData *GpsAlm ,unsigned char idofsubframe)
{
	char SVID;
	char PageType;

	switch(idofsubframe)
	{
	case 4:
		SVID = ((word[2]&0x003f0000) >> 16)&0xff;
		if((SVID>=25) && (SVID<=32))
		{
//            GpsAlm->almPRNs[SVID].PRN = SVID;
//            GpsAlm->almPRNs[SVID].xmit_time = (long) ((word[1] & 0x00FFFF80) >> 7);
//            GpsAlm->almPRNs[SVID].ecc = ldexp((double) ((long) (word[2] & 0x0000FFFF)), -21);
//            GpsAlm->almPRNs[SVID].Toa = (DWORD) ((word[3] & 0x00FF0000) >> 4);
//            GpsAlm->almPRNs[SVID].i_offset = ldexp((double) ((short) (word[3] & 0x0000FFFF)), -19);
//            GpsAlm->almPRNs[SVID].OMEGAdot = ldexp((double) ((short) ((word[4] & 0x00FFFF00) >> 8)), -38);
//            GpsAlm->almPRNs[SVID].SV_health = (BYTE) (word[4] & 0x000000FF);
//            GpsAlm->almPRNs[SVID].Asqrt = ldexp((double) ((DWORD) (word[5] & 0x00FFFFFF)), -11);
//            GpsAlm->almPRNs[SVID].OMEGA0 = ldexp((double) ((long) ((word[6] & 0x00FFFFFF) << 8)), -(23 + 8));
//            GpsAlm->almPRNs[SVID].w = ldexp((double) ((long) ((word[7] & 0x00FFFFFF) << 8)), -(23 + 8));
//            GpsAlm->almPRNs[SVID].M0 = ldexp((double) ((long) ((word[8] & 0x00FFFFFF) << 8)), -(23 + 8));
//            GpsAlm->almPRNs[SVID].AF0 = ldexp((double) ((short) (((word[9] & 0x00FF0000) >> 13 | (word[9] & 0x0000001C) >> 2) << 5)), -(20 + 5));
//            GpsAlm->almPRNs[SVID].AF1 = ldexp((double) ((short) (word[9] & 0x0000FFE0)), -(38 + 3));
		}
		else
		{
			PageType = GPSPatID[SVID-51];
			switch(PageType)
			{
				case 6: // Page52 contain ERD info but return 6 here
				case 7:
				/// ignore page becase no data for our structure
				break;

				case 8: /// Page with UTC and ionosphere parameters
				GpsAlm->alpha[0] = ldexp((double) ((char) ((word[2] & 0x0000FF00) >> 8)), -30);
				GpsAlm->alpha[1] = ldexp((double) ((char) (word[2] & 0x000000FF)), -27);
				GpsAlm->alpha[2] = ldexp((double) ((char) ((word[3] & 0x00FF0000) >> 16)), -24);
				GpsAlm->alpha[3] = ldexp((double) ((char) ((word[3] & 0x0000FF00) >> 8)), -24);
				GpsAlm->beta[0] = ldexp((double) ((char) (word[3] & 0x000000FF)), 11);
				GpsAlm->beta[1] = ldexp((double) ((char) ((word[4] & 0x00FF0000) >> 16)), 14);
				GpsAlm->beta[2] = ldexp((double) ((char) ((word[4] & 0x0000FF00) >> 8)), 16);
				GpsAlm->beta[3] = ldexp((double) ((char) (word[4] & 0x000000FF)), 16);
//                snprintf(test_str, 512, "aplha0:%f aplha1:%f\r\n", GpsAlm->alpha[0]*1e30, GpsAlm->alpha[1]*1e30);
//                USR_Ser_WrStr (test_str, BSP_SER_COMM_UART_03);
//                GpsAlm->A0 = ldexp((double) ((long) (((word[6] & 0x00FFFFFF) << 8) | ((word[7] & 0x00FF0000) >> 16))), -30);
//                GpsAlm->A1 = ldexp((double) ((long) ((word[5] & 0x00FFFFFF) << 8)), -(50+8));
//                GpsAlm->dt_ls = (char) ((word[8] & 0x00FF0000) >> 16);
//                GpsAlm->t_ot = (DWORD) ((word[7] & 0x0000FF00) << 4);
//                GpsAlm->wn_t = (BYTE) (word[7] & 0x000000FF);
//                GpsAlm->wn_lsf = (BYTE) ((word[8] & 0x0000FF00) >> 8);
//                GpsAlm->dn = (BYTE) (word[8] & 0x000000FF);;
//                GpsAlm->dt_lsf = (char) ((word[9] & 0x00FF0000) >> 16);
//                GpsAlm->haveUTC = true;
				break;

				case 9:  /// Page with Health for 25-32 and AS/SV config
//                for(i=0; i < 32; i++)
//                    GpsAlm->SV_config[i] = (BYTE) ((word[3+(i+2)/6] >> (20-((i+2)%6)*4)) & 0x0000000F);
//                for(i=24; i < 32; i++)
//                    GpsAlm->health[i] = (BYTE) ((word[1+(i+3)/4] >> (18-((i+3)%4)*6)) & 0x0000003F);
				break;

				case 10:/// Page with Special Message Page 55
//                for(i=0;i < 22; i++)
//                    GpsAlm->special_msg[i] = (BYTE) ((word[3+i/4] >> (18-(i%4)*6)) & 0x0000003F);
				break;
				default: break;
			}
		}
		break;

	case 5:
//        SVID = ((word[2] & 0x003F0000) >> 16)&0xff;
//
//        if((SVID>=1) && (SVID<=24))
//        {
//            GpsAlm->almPRNs[SVID].PRN = SVID;
//            GpsAlm->almPRNs[SVID].xmit_time = (long) ((word[1] & 0x00FFFF80) >> 7);
//            GpsAlm->almPRNs[SVID].ecc = ldexp((double) ((long) (word[2] & 0x0000FFFF)), -21);
//            GpsAlm->almPRNs[SVID].Toa = (DWORD) ((word[3] & 0x00FF0000) >> 4);
//            GpsAlm->almPRNs[SVID].i_offset = ldexp((double) ((short) (word[3] & 0x0000FFFF)), -19);
//            GpsAlm->almPRNs[SVID].OMEGAdot = ldexp((double) ((short) ((word[4] & 0x00FFFF00) >> 8)), -38);
//            GpsAlm->almPRNs[SVID].SV_health = (BYTE) (word[4] & 0x000000FF);
//            GpsAlm->almPRNs[SVID].Asqrt = ldexp((double) ((DWORD) (word[5] & 0x00FFFFFF)), -11);
//            GpsAlm->almPRNs[SVID].OMEGA0 = ldexp((double) ((long) ((word[6] & 0x00FFFFFF) << 8)), -(23 + 8));
//            GpsAlm->almPRNs[SVID].w = ldexp((double) ((long) ((word[7] & 0x00FFFFFF) << 8)), -(23 + 8));
//            GpsAlm->almPRNs[SVID].M0 = ldexp((double) ((long) ((word[8] & 0x00FFFFFF) << 8)), -(23 + 8));
//            GpsAlm->almPRNs[SVID].AF0 = ldexp((double) ((short) (((word[9] & 0x00FF0000) >> 13 | (word[9] & 0x0000001C) >> 2) << 5)), -(20 + 5));
//            GpsAlm->almPRNs[SVID].AF1 = ldexp((double) ((short) (word[9] & 0x0000FFE0)), -(38 + 3));
//        }
//        else if (SVID==51)
//        {
//            for(i=0; i < 24; i++)
//                GpsAlm->health[i] = (BYTE) ((word[3+i/4] >> (18-(i%4)*6)) & 0x0000003F);
//            // set the t_oa for this record
//            GpsAlm->t_oa = (DWORD) ((word[2] & 0x0000FF00) << 4);
//            GpsAlm->wn_a = (BYTE) (word[2] & 0x000000FF);
//        }
		break;

	default:
		break;
	}
	return 1;
}




void    GPSMeanAnomaly_LNAV(int prnnum)
{
	// calculate kepler's equation for eccentric anomaly
	GPSL_EphCal.CorrMeanMotion = sqrt(WGS_GRAPARA) / pow(GpsEphmeris_LNav[prnnum].Asqrt, 3.0) + GpsEphmeris_LNav[prnnum].dn;
	// double tEphRef
	GPSL_EphCal.elapte = substract_timer_double(databuff_obsbuffdata[prnnum].tTrans, format_timer(GpsEphmeris_LNav[prnnum].Toe));
	GPSL_EphCal.elapte = checktime(GPSL_EphCal.elapte);
	if (GPSL_EphCal.elapte > 302400)
	{
		GPSL_EphCal.elapte -= 604800;
	}
	else if (GPSL_EphCal.elapte < -302400)
	{
		GPSL_EphCal.elapte += 604800;
	}
	if (GpsEphmeris_LNav[prnnum].Asqrt < 2550)
	{
		GPSL_EphCal.MeanAnomaly = fmod(GpsEphmeris_LNav[prnnum].M0, 2 * M_PI);
		GPSL_EphCal.lecc = 0;
	}
	else
	{
		GPSL_EphCal.MeanAnomaly = fmod(GpsEphmeris_LNav[prnnum].M0 + GPSL_EphCal.CorrMeanMotion * GPSL_EphCal.elapte, 2 * M_PI);
		GPSL_EphCal.lecc = GpsEphmeris_LNav[prnnum].ecc;
	}
	GPSL_EphCal.Ek = GPSL_EphCal.MeanAnomaly + GPSL_EphCal.lecc * sin(GPSL_EphCal.MeanAnomaly);
	GPSL_EphCal.LoopCnt = 0;
	do
	{
		GPSL_EphCal.dEk = GPSL_EphCal.MeanAnomaly - (GPSL_EphCal.Ek - GPSL_EphCal.lecc * sin(GPSL_EphCal.Ek));
		GPSL_EphCal.Ek += GPSL_EphCal.dEk;
		GPSL_EphCal.LoopCnt++;
	} while ((fabs(GPSL_EphCal.dEk) > 1.0e-15) && (GPSL_EphCal.LoopCnt < 10));
	GPSL_EphCal.TempData = substract_timer_double(databuff_obsbuffdata[prnnum].tTrans, format_timer(GpsEphmeris_LNav[prnnum].Toc));
	GPSL_EphCal.TempData = checktime(GPSL_EphCal.TempData);
	if (GPSL_EphCal.TempData > 302400)
	{
		GPSL_EphCal.TempData -= 604800;
	}
	else if (GPSL_EphCal.TempData < -302400)
	{
		GPSL_EphCal.TempData += 604800;
	}
	// Time correction
	databuff_obsbuffdata[prnnum].SVloc.dt = GpsEphmeris_LNav[prnnum].af0
		+ GpsEphmeris_LNav[prnnum].af1 * GPSL_EphCal.TempData
		+ GpsEphmeris_LNav[prnnum].af2 * pow(GPSL_EphCal.TempData, 2.0);
	databuff_obsbuffdata[prnnum].SVloc.dt -= GpsEphmeris_LNav[prnnum].Tgd;
	//    databuff_obsbuffdata[prnnum].SVloc.dt = 0;
	GPSL_EphCal.dtr = CONST_F * GPSL_EphCal.lecc * GpsEphmeris_LNav[prnnum].Asqrt;
	GPSL_EphCal.dtr *= sin(GPSL_EphCal.Ek);
	databuff_obsbuffdata[prnnum].SVloc.dt += GPSL_EphCal.dtr;
}


void    SVpos_GPS_LNAV(int prnnum)
{
	memset(&GPSL_EphCal, 0, sizeof(GPSL_EphCal));
	if (prnnum > GpsPrnNum || prnnum <= 0)       // error prn
	{
		return;
	}
	GPSMeanAnomaly_LNAV(prnnum);
	databuff_obsbuffdata[prnnum].tTrans = substract_timer(databuff_obsbuffdata[prnnum].TsvSend, format_timer(databuff_obsbuffdata[prnnum].SVloc.dt));//修正发送时间
	GPSMeanAnomaly_LNAV(prnnum);
	databuff_obsbuffdata[prnnum].tTrans = substract_timer(databuff_obsbuffdata[prnnum].TsvSend, format_timer(databuff_obsbuffdata[prnnum].SVloc.dt));//修正发送时间
	// Eph -> SVPos
	// double TrueAnomaly
	GPSL_EphCal.TrueAnomaly = atan2(sqrt(1 - pow(GPSL_EphCal.lecc, 2.0)) * sin(GPSL_EphCal.Ek), cos(GPSL_EphCal.Ek) - GPSL_EphCal.lecc);
	GPSL_EphCal.ArgofLatitude = GPSL_EphCal.TrueAnomaly + GpsEphmeris_LNav[prnnum].w;
	GPSL_EphCal.tArgofLatitude = 2 * GPSL_EphCal.ArgofLatitude;
	// double dArgofLatitude
	GPSL_EphCal.Lat = GpsEphmeris_LNav[prnnum].Cus * sin(GPSL_EphCal.tArgofLatitude) + GpsEphmeris_LNav[prnnum].Cuc * cos(GPSL_EphCal.tArgofLatitude);
	// double dRadius
	GPSL_EphCal.Rad = GpsEphmeris_LNav[prnnum].Crs * sin(GPSL_EphCal.tArgofLatitude) + GpsEphmeris_LNav[prnnum].Crc * cos(GPSL_EphCal.tArgofLatitude);
	// double dInclination
	GPSL_EphCal.Inc = GpsEphmeris_LNav[prnnum].Cis * sin(GPSL_EphCal.tArgofLatitude) + GpsEphmeris_LNav[prnnum].Cic * cos(GPSL_EphCal.tArgofLatitude);
	// double CorrLatitude
	GPSL_EphCal.Lat += GPSL_EphCal.ArgofLatitude;
	// double CorrRadius
	GPSL_EphCal.Rad += pow(GpsEphmeris_LNav[prnnum].Asqrt, 2.0) * (1 - GPSL_EphCal.lecc * cos(GPSL_EphCal.Ek));
	// double CorrInclination
	if (GpsEphmeris_LNav[prnnum].Asqrt < 2550)
	{
		GPSL_EphCal.Inc += GpsEphmeris_LNav[prnnum].i0;
		GPSL_EphCal.CorrLonAscending = GpsEphmeris_LNav[prnnum].OMEGA0 - GpsEphmeris_LNav[prnnum].OMEGAdot * GpsEphmeris_LNav[prnnum].Toe;
	}
	else
	{
		GPSL_EphCal.Inc += GpsEphmeris_LNav[prnnum].i0 + GpsEphmeris_LNav[prnnum].idot * GPSL_EphCal.elapte;
		GPSL_EphCal.CorrLonAscending = GpsEphmeris_LNav[prnnum].OMEGA0 + (GpsEphmeris_LNav[prnnum].OMEGAdot - WGS_ROTATEVEL) * GPSL_EphCal.elapte - WGS_ROTATEVEL * GpsEphmeris_LNav[prnnum].Toe;
	}
	GPSL_EphCal.xOrbital = GPSL_EphCal.Rad * cos(GPSL_EphCal.Lat);
	GPSL_EphCal.yOrbital = GPSL_EphCal.Rad * sin(GPSL_EphCal.Lat);
	// ECEF Corrdinate
	databuff_obsbuffdata[prnnum].SVloc.x = GPSL_EphCal.xOrbital * cos(GPSL_EphCal.CorrLonAscending) - GPSL_EphCal.yOrbital * cos(GPSL_EphCal.Inc) * sin(GPSL_EphCal.CorrLonAscending);
	databuff_obsbuffdata[prnnum].SVloc.y = GPSL_EphCal.xOrbital * sin(GPSL_EphCal.CorrLonAscending) + GPSL_EphCal.yOrbital * cos(GPSL_EphCal.Inc) * cos(GPSL_EphCal.CorrLonAscending);
	databuff_obsbuffdata[prnnum].SVloc.z = GPSL_EphCal.yOrbital * sin(GPSL_EphCal.Inc);
	databuff_obsbuffdata[prnnum].beChanPVT = true;
	// sv velocity calculation.
	GPSL_EphCal.DotEk = GPSL_EphCal.CorrMeanMotion / (1 - GPSL_EphCal.lecc * cos(GPSL_EphCal.Ek));
	GPSL_EphCal.DotTrueAnomaly = sqrt(1 - pow(GPSL_EphCal.lecc, 2.0)) * GPSL_EphCal.DotEk / (1 - GPSL_EphCal.lecc * cos(GPSL_EphCal.Ek)); //0712
	//GPSL_EphCal.DotTrueAnomaly = sqrt((1+GPSL_EphCal.lecc) / (1+GPSL_EphCal.lecc)) * pow(cos(GPSL_EphCal.TrueAnomaly/2),2.0) / pow(cos(GPSL_EphCal.Ek/2),2.0) * GPSL_EphCal.DotEk;
	GPSL_EphCal.DotTempData = GPSL_EphCal.DotTrueAnomaly * 2.0;
	GPSL_EphCal.DotIncCorr = GPSL_EphCal.DotTempData * (GpsEphmeris_LNav[prnnum].Cis * cos(GPSL_EphCal.tArgofLatitude) - GpsEphmeris_LNav[prnnum].Cic * sin(GPSL_EphCal.tArgofLatitude));
	GPSL_EphCal.DotRadCorr = GPSL_EphCal.DotTempData * (GpsEphmeris_LNav[prnnum].Crs * cos(GPSL_EphCal.tArgofLatitude) - GpsEphmeris_LNav[prnnum].Crc * sin(GPSL_EphCal.tArgofLatitude));
	GPSL_EphCal.DotLatCorr = GPSL_EphCal.DotTempData * (GpsEphmeris_LNav[prnnum].Cus * cos(GPSL_EphCal.tArgofLatitude) - GpsEphmeris_LNav[prnnum].Cuc * sin(GPSL_EphCal.tArgofLatitude));
	GPSL_EphCal.DotCorrLonAscending = GpsEphmeris_LNav[prnnum].OMEGAdot - WGS_ROTATEVEL;
	GPSL_EphCal.DotInc = GpsEphmeris_LNav[prnnum].idot + GPSL_EphCal.DotIncCorr;
	GPSL_EphCal.DotRad = pow(GpsEphmeris_LNav[prnnum].Asqrt, 2.0) * GPSL_EphCal.lecc * GPSL_EphCal.DotEk;
	GPSL_EphCal.DotRad = GPSL_EphCal.DotRad * sin(GPSL_EphCal.Ek) + GPSL_EphCal.DotRadCorr;
	GPSL_EphCal.DotLat = GPSL_EphCal.DotTrueAnomaly + GPSL_EphCal.DotLatCorr;
	GPSL_EphCal.DotxOrbital = GPSL_EphCal.DotRad * cos(GPSL_EphCal.Lat) - GPSL_EphCal.Rad * GPSL_EphCal.DotLat * sin(GPSL_EphCal.Lat);
	GPSL_EphCal.DotyOrbital = GPSL_EphCal.DotRad * sin(GPSL_EphCal.Lat) + GPSL_EphCal.Rad * GPSL_EphCal.DotLat * cos(GPSL_EphCal.Lat);
	// ECEF Corrdinate
	//  databuff_obsbuffdata[prnnum].SVloc.Vx = GPSL_EphCal.DotxOrbital * cos(GPSL_EphCal.CorrLonAscending) - databuff_obsbuffdata[prnnum].SVloc.y * GPSL_EphCal.DotCorrLonAscending - (GPSL_EphCal.DotyOrbital * cos(GPSL_EphCal.Inc) - databuff_obsbuffdata[prnnum].SVloc.z * GPSL_EphCal.DotInc) * sin(GPSL_EphCal.CorrLonAscending);
	//  databuff_obsbuffdata[prnnum].SVloc.Vy = databuff_obsbuffdata[prnnum].SVloc.x * GPSL_EphCal.DotCorrLonAscending + (GPSL_EphCal.DotyOrbital * cos(GPSL_EphCal.Inc) - databuff_obsbuffdata[prnnum].SVloc.z * GPSL_EphCal.DotInc) * cos(GPSL_EphCal.CorrLonAscending) + GPSL_EphCal.DotxOrbital * sin(GPSL_EphCal.CorrLonAscending);
	//  databuff_obsbuffdata[prnnum].SVloc.Vz = GPSL_EphCal.DotyOrbital * sin(GPSL_EphCal.Inc) + GPSL_EphCal.yOrbital * GPSL_EphCal.DotInc * cos(GPSL_EphCal.Inc);
	GPSL_EphCal.R_LonAscending[0] = cos(GPSL_EphCal.CorrLonAscending);
	GPSL_EphCal.R_LonAscending[1] = -sin(GPSL_EphCal.CorrLonAscending);
	GPSL_EphCal.R_LonAscending[3] = -GPSL_EphCal.R_LonAscending[1];
	GPSL_EphCal.R_LonAscending[4] = GPSL_EphCal.R_LonAscending[0];
	GPSL_EphCal.R_LonAscending[8] = 1;
	GPSL_EphCal.Rdot_LonAscending[0] = -GPSL_EphCal.DotCorrLonAscending * sin(GPSL_EphCal.CorrLonAscending);
	GPSL_EphCal.Rdot_LonAscending[1] = -GPSL_EphCal.DotCorrLonAscending * cos(GPSL_EphCal.CorrLonAscending);
	GPSL_EphCal.Rdot_LonAscending[3] = -GPSL_EphCal.Rdot_LonAscending[1];
	GPSL_EphCal.Rdot_LonAscending[4] = GPSL_EphCal.Rdot_LonAscending[0];
	GPSL_EphCal.R_Inc[0] = 1;
	GPSL_EphCal.R_Inc[4] = cos(GPSL_EphCal.Inc);
	GPSL_EphCal.R_Inc[5] = -sin(GPSL_EphCal.Inc);
	GPSL_EphCal.R_Inc[7] = -GPSL_EphCal.R_Inc[5];
	GPSL_EphCal.R_Inc[8] = GPSL_EphCal.R_Inc[4];
	GPSL_EphCal.Rdot_Inc[4] = -GPSL_EphCal.DotInc * sin(GPSL_EphCal.Inc);
	GPSL_EphCal.Rdot_Inc[5] = -GPSL_EphCal.DotInc * cos(GPSL_EphCal.Inc);
	GPSL_EphCal.Rdot_Inc[7] = -GPSL_EphCal.Rdot_Inc[5];
	GPSL_EphCal.Rdot_Inc[8] = GPSL_EphCal.Rdot_Inc[4];
	GPSL_EphCal.r_orb[0] = GPSL_EphCal.xOrbital;
	GPSL_EphCal.r_orb[1] = GPSL_EphCal.yOrbital;
	GPSL_EphCal.rdot_orb[0] = GPSL_EphCal.DotxOrbital;
	GPSL_EphCal.rdot_orb[1] = GPSL_EphCal.DotyOrbital;
	MatrixMult(GPSL_EphCal.R_ECEF, GPSL_EphCal.R_LonAscending, GPSL_EphCal.R_Inc, 3, 3, 3, 3);
	MatrixMult(GPSL_EphCal.tmp_mat, GPSL_EphCal.Rdot_LonAscending, GPSL_EphCal.R_Inc, 3, 3, 3, 3);
	MatrixMult(GPSL_EphCal.Rdot_ECEF, GPSL_EphCal.R_LonAscending, GPSL_EphCal.Rdot_Inc, 3, 3, 3, 3);
	MatrixAdd(GPSL_EphCal.Rdot_ECEF, GPSL_EphCal.Rdot_ECEF, GPSL_EphCal.tmp_mat, 3, 3);
	MatrixMult(GPSL_EphCal.tmp_mat, GPSL_EphCal.Rdot_ECEF, GPSL_EphCal.r_orb, 3, 3, 3, 1);
	MatrixMult(GPSL_EphCal.vel, GPSL_EphCal.R_ECEF, GPSL_EphCal.rdot_orb, 3, 3, 3, 1);
	MatrixAdd(GPSL_EphCal.vel, GPSL_EphCal.vel, GPSL_EphCal.tmp_mat, 3, 1);
	databuff_obsbuffdata[prnnum].SVloc.Vx = GPSL_EphCal.vel[0];
	databuff_obsbuffdata[prnnum].SVloc.Vy = GPSL_EphCal.vel[1];
	databuff_obsbuffdata[prnnum].SVloc.Vz = GPSL_EphCal.vel[2];
	//  if (prnnum ==17 || prnnum ==9 || prnnum ==23 )
	//{snprintf(test_str, 512, " SV%d %d %10.4f, %10.4f, %10.4f, ",  prnnum,prnnum,databuff_obsbuffdata[prnnum].SVloc.Vx,databuff_obsbuffdata[prnnum].SVloc.Vy,databuff_obsbuffdata[prnnum].SVloc.Vz);
	//USR_Ser_WrStr (test_str, BSP_SER_COMM_UART_03);
	//}
};




void    BD2MeanAnomaly(int prnnum)
{
	// calculate kepler's equation for eccentric anomaly
	BD2_EphCal.CorrMeanMotion = sqrt(CGS_GRAPARA) / BD2_EphCal.A3_2 + BD2Ephmeris_B1[prnnum].dn;                                    // 骞冲潎瑙掗€熷害+鏀规�ｉ噺=鏀规�ｅ钩鍧囪�掗€熷害
	// double tEphRef
	BD2_EphCal.elapte = substract_timer_double(databuff_obsbuffdata[prnnum].tTrans, format_timer(BD2Ephmeris_B1[prnnum].Toe));        // 鍘嗗厓鍒板弬鑰冨巻鍏冩椂闂村樊
	BD2_EphCal.elapte = checktime(BD2_EphCal.elapte);
	if (BD2_EphCal.elapte > 302400)
	{
		BD2_EphCal.elapte -= 604800;
	}
	else if (BD2_EphCal.elapte < -302400)
	{
		BD2_EphCal.elapte += 604800;
	}
	/*  if(BD2_EphCal.A1_2 < 2550)
		{
			BD2_EphCal.MeanAnomaly = fmod(BD2Ephmeris_B1[prnnum].M0, 2*M_PI);
			BD2_EphCal.lecc = 0;
		}
		else
		{*/
	BD2_EphCal.MeanAnomaly = fmod(BD2Ephmeris_B1[prnnum].M0 + BD2_EphCal.CorrMeanMotion * BD2_EphCal.elapte, 2 * M_PI);         // 璁＄畻骞宠繎鐐硅��
	BD2_EphCal.lecc = BD2Ephmeris_B1[prnnum].ecc;
	//  }
	BD2_EphCal.Ek = BD2_EphCal.MeanAnomaly + BD2_EphCal.lecc * sin(BD2_EphCal.MeanAnomaly);
	BD2_EphCal.LoopCnt = 0;
	do
	{
		BD2_EphCal.dEk = BD2_EphCal.MeanAnomaly - (BD2_EphCal.Ek - BD2_EphCal.lecc * sin(BD2_EphCal.Ek));
		BD2_EphCal.Ek += BD2_EphCal.dEk;
		BD2_EphCal.LoopCnt++;
	} while ((fabs(BD2_EphCal.dEk) > 1.0e-15) && (BD2_EphCal.LoopCnt < 10));                                                        // 杩�浠ｈ�＄畻鍋忚繎鐐硅��
	BD2_EphCal.TempData = substract_timer_double(databuff_obsbuffdata[prnnum].tTrans, format_timer(BD2Ephmeris_B1[prnnum].Toc));
	BD2_EphCal.TempData = checktime(BD2_EphCal.TempData);
	if (BD2_EphCal.TempData > 302400)
	{
		BD2_EphCal.TempData -= 604800;
	}
	else if (BD2_EphCal.TempData < -302400)
	{
		BD2_EphCal.TempData += 604800;
	}
	// Time correction
	databuff_obsbuffdata[prnnum].SVloc.dt = BD2Ephmeris_B1[prnnum].a0
		+ BD2Ephmeris_B1[prnnum].a1 * BD2_EphCal.TempData
		+ BD2Ephmeris_B1[prnnum].a2 * pow(BD2_EphCal.TempData, 2.0);                                                                // 鍗�鏄熼挓宸�鏍℃��
	databuff_obsbuffdata[prnnum].SVloc.dt -= BD2Ephmeris_B1[prnnum].Tgd1;
	BD2_EphCal.dtr = CONST_F * BD2_EphCal.lecc * BD2_EphCal.A1_2;
	BD2_EphCal.dtr *= sin(BD2_EphCal.Ek);                                                                                       // 鍗�鏄熼挓宸�鐩稿�硅�烘牎姝ｉ」
	databuff_obsbuffdata[prnnum].SVloc.dt += BD2_EphCal.dtr;
}

void    SVpos_BD2(int prnnum)
{
	memset(&BD2_EphCal, 0, sizeof(BD2_EphCal));
	if (prnnum > BD2PrnNum || prnnum <= 0)       // error prn
	{
		return;
	}
	BD2_EphCal.A1_2 = BD2Ephmeris_B1[prnnum].Asqrt;
	BD2_EphCal.A = BD2_EphCal.A1_2 * BD2_EphCal.A1_2;
	BD2_EphCal.A3_2 = BD2_EphCal.A * BD2_EphCal.A1_2;
	BD2MeanAnomaly(prnnum);
	databuff_obsbuffdata[prnnum].tTrans = substract_timer(databuff_obsbuffdata[prnnum].TsvSend, format_timer(databuff_obsbuffdata[prnnum].SVloc.dt));//淇�姝ｅ彂閫佹椂闂�
	BD2MeanAnomaly(prnnum);
	databuff_obsbuffdata[prnnum].tTrans = substract_timer(databuff_obsbuffdata[prnnum].TsvSend, format_timer(databuff_obsbuffdata[prnnum].SVloc.dt));//淇�姝ｅ彂閫佹椂闂�
	// Eph -> SVPos
	// double TrueAnomaly
	BD2_EphCal.TrueAnomaly = atan2(sqrt(1 - pow(BD2_EphCal.lecc, 2.0)) * sin(BD2_EphCal.Ek), cos(BD2_EphCal.Ek) - BD2_EphCal.lecc);         // 璁＄畻鐪熻繎鐐硅��
	BD2_EphCal.ArgofLatitude = BD2_EphCal.TrueAnomaly + BD2Ephmeris_B1[prnnum].w;                                                               // 绾�搴﹀箙瑙掑弬鏁�
	BD2_EphCal.tArgofLatitude = 2 * BD2_EphCal.ArgofLatitude;
	// double dArgofLatitude
	BD2_EphCal.Lat = BD2Ephmeris_B1[prnnum].Cus * sin(BD2_EphCal.tArgofLatitude) + BD2Ephmeris_B1[prnnum].Cuc * cos(BD2_EphCal.tArgofLatitude);     // 绾�搴﹀箙瑙掓敼姝ｉ」
	// double dRadius
	BD2_EphCal.Rad = BD2Ephmeris_B1[prnnum].Crs * sin(BD2_EphCal.tArgofLatitude) + BD2Ephmeris_B1[prnnum].Crc * cos(BD2_EphCal.tArgofLatitude);     // 寰勫悜鏀规�ｉ」
	// double dInclination
	BD2_EphCal.Inc = BD2Ephmeris_B1[prnnum].Cis * sin(BD2_EphCal.tArgofLatitude) + BD2Ephmeris_B1[prnnum].Cic * cos(BD2_EphCal.tArgofLatitude);     // 杞ㄩ亾鍊捐�掓敼姝ｉ」
	// double CorrLatitude
	BD2_EphCal.Lat += BD2_EphCal.ArgofLatitude;                                                                                             // 鏀规�ｅ悗鐨勭含搴﹀箙瑙�
	// double CorrRadius
	BD2_EphCal.Rad += BD2_EphCal.A * (1 - BD2_EphCal.lecc * cos(BD2_EphCal.Ek));                                                            // 鏀规�ｅ悗鐨勫緞鍚�
	// double CorrInclination
	/*  if(BD2_EphCal.A1_2 < 2550)
		{
			BD2_EphCal.Inc += BD2Ephmeris_B1[prnnum].i0;
			BD2_EphCal.CorrLonAscending = BD2Ephmeris_B1[prnnum].OMEGA0 - BD2Ephmeris_B1[prnnum].OMEGAdot * BD2Ephmeris_B1[prnnum].Toe;
		}
		else
		{*/
	BD2_EphCal.Inc += BD2Ephmeris_B1[prnnum].i0 + BD2Ephmeris_B1[prnnum].idot * BD2_EphCal.elapte;                                              // 鏀规�ｅ悗鐨勮建閬撳€捐��
	//  }
	BD2_EphCal.xOrbital = BD2_EphCal.Rad * cos(BD2_EphCal.Lat);                                                                             // 鍗�鏄熷湪杞ㄩ亾骞抽潰鍐呯殑鍧愭爣
	BD2_EphCal.yOrbital = BD2_EphCal.Rad * sin(BD2_EphCal.Lat);
	if (prnnum <= 5)                                                                                                                         // 鍘嗗厓鍗囦氦鐐硅丹缁忥紙鎯�鎬х郴锛�
	{
		BD2_EphCal.CorrLonAscending = BD2Ephmeris_B1[prnnum].OMEGA0 + BD2Ephmeris_B1[prnnum].OMEGAdot * BD2_EphCal.elapte - CGS_ROTATEVEL * BD2Ephmeris_B1[prnnum].Toe;
	}
	else                                                                                                                                    // 鍘嗗厓鍗囦氦鐐硅丹缁忥紙鍦板浐绯伙級
	{
		BD2_EphCal.CorrLonAscending = BD2Ephmeris_B1[prnnum].OMEGA0 + (BD2Ephmeris_B1[prnnum].OMEGAdot - CGS_ROTATEVEL) * BD2_EphCal.elapte - CGS_ROTATEVEL * BD2Ephmeris_B1[prnnum].Toe;
	}
	// CGS2000 Corrdinate
	BD2_EphCal.x_gk = BD2_EphCal.xOrbital * cos(BD2_EphCal.CorrLonAscending) - BD2_EphCal.yOrbital * cos(BD2_EphCal.Inc) * sin(BD2_EphCal.CorrLonAscending);
	BD2_EphCal.y_gk = BD2_EphCal.xOrbital * sin(BD2_EphCal.CorrLonAscending) + BD2_EphCal.yOrbital * cos(BD2_EphCal.Inc) * cos(BD2_EphCal.CorrLonAscending);
	BD2_EphCal.z_gk = BD2_EphCal.yOrbital * sin(BD2_EphCal.Inc);
	BD2_EphCal.TempData = CGS_ROTATEVEL * BD2_EphCal.elapte;
	if (prnnum <= 5)
	{
		databuff_obsbuffdata[prnnum].SVloc.x = cos(BD2_EphCal.TempData) * BD2_EphCal.x_gk + sin(BD2_EphCal.TempData) * CGS_COS_N5 * BD2_EphCal.y_gk + sin(BD2_EphCal.TempData) * CGS_SIN_N5 * BD2_EphCal.z_gk;
		databuff_obsbuffdata[prnnum].SVloc.y = -sin(BD2_EphCal.TempData) * BD2_EphCal.x_gk + cos(BD2_EphCal.TempData) * CGS_COS_N5 * BD2_EphCal.y_gk + cos(BD2_EphCal.TempData) * CGS_SIN_N5 * BD2_EphCal.z_gk;
		databuff_obsbuffdata[prnnum].SVloc.z = -CGS_SIN_N5 * BD2_EphCal.y_gk + CGS_COS_N5 * BD2_EphCal.z_gk;
	}
	else
	{
		databuff_obsbuffdata[prnnum].SVloc.x = BD2_EphCal.x_gk;
		databuff_obsbuffdata[prnnum].SVloc.y = BD2_EphCal.y_gk;
		databuff_obsbuffdata[prnnum].SVloc.z = BD2_EphCal.z_gk;
	}
	//  if(prnnum <= 5)//0627 close geo
	//      databuff_obsbuffdata[prnnum].beChanPVT = false;
	//  else
	databuff_obsbuffdata[prnnum].beChanPVT = true;
	// sv velocity calculation.
	BD2_EphCal.DotEk = BD2_EphCal.CorrMeanMotion / (1 - BD2_EphCal.lecc * cos(BD2_EphCal.Ek));
	BD2_EphCal.DotTrueAnomaly = sqrt(1 - BD2_EphCal.lecc * BD2_EphCal.lecc) * BD2_EphCal.DotEk / (1 - BD2_EphCal.lecc * cos(BD2_EphCal.Ek)); //0712
	//BD2_EphCal.DotTrueAnomaly = sqrt((1+BD2_EphCal.lecc) / (1+BD2_EphCal.lecc)) * pow(cos(BD2_EphCal.TrueAnomaly/2),2.0) / pow(cos(BD2_EphCal.Ek/2),2.0) * BD2_EphCal.DotEk;
	BD2_EphCal.DotTempData = BD2_EphCal.DotTrueAnomaly * 2.0;
	BD2_EphCal.DotIncCorr = BD2_EphCal.DotTempData * (BD2Ephmeris_B1[prnnum].Cis * cos(BD2_EphCal.tArgofLatitude) - BD2Ephmeris_B1[prnnum].Cic * sin(BD2_EphCal.tArgofLatitude));
	BD2_EphCal.DotRadCorr = BD2_EphCal.DotTempData * (BD2Ephmeris_B1[prnnum].Crs * cos(BD2_EphCal.tArgofLatitude) - BD2Ephmeris_B1[prnnum].Crc * sin(BD2_EphCal.tArgofLatitude));
	BD2_EphCal.DotLatCorr = BD2_EphCal.DotTempData * (BD2Ephmeris_B1[prnnum].Cus * cos(BD2_EphCal.tArgofLatitude) - BD2Ephmeris_B1[prnnum].Cuc * sin(BD2_EphCal.tArgofLatitude));
	if (prnnum <= 5)
	{
		BD2_EphCal.DotCorrLonAscending = BD2Ephmeris_B1[prnnum].OMEGAdot;
	}
	else
	{
		BD2_EphCal.DotCorrLonAscending = BD2Ephmeris_B1[prnnum].OMEGAdot - CGS_ROTATEVEL;
	}
	BD2_EphCal.DotInc = BD2Ephmeris_B1[prnnum].idot + BD2_EphCal.DotIncCorr;
	BD2_EphCal.DotRad = BD2_EphCal.A * BD2_EphCal.lecc * BD2_EphCal.DotEk * sin(BD2_EphCal.Ek) + BD2_EphCal.DotRadCorr;
	BD2_EphCal.DotLat = BD2_EphCal.DotTrueAnomaly + BD2_EphCal.DotLatCorr;
	BD2_EphCal.DotxOrbital = BD2_EphCal.DotRad * cos(BD2_EphCal.Lat) - BD2_EphCal.Rad * BD2_EphCal.DotLat * sin(BD2_EphCal.Lat);
	BD2_EphCal.DotyOrbital = BD2_EphCal.DotRad * sin(BD2_EphCal.Lat) + BD2_EphCal.Rad * BD2_EphCal.DotLat * cos(BD2_EphCal.Lat);
	// CGS2000 Corrdinate
	//  BD2_EphCal.vx_gk = BD2_EphCal.DotxOrbital * cos(BD2_EphCal.CorrLonAscending) - BD2_EphCal.y_gk * BD2_EphCal.DotCorrLonAscending - (BD2_EphCal.DotyOrbital * cos(BD2_EphCal.Inc) - BD2_EphCal.z_gk * BD2_EphCal.DotInc) * sin(BD2_EphCal.CorrLonAscending);
	//  BD2_EphCal.vy_gk = BD2_EphCal.x_gk * BD2_EphCal.DotCorrLonAscending + (BD2_EphCal.DotyOrbital * cos(BD2_EphCal.Inc) - BD2_EphCal.z_gk * BD2_EphCal.DotInc) * cos(BD2_EphCal.CorrLonAscending) + BD2_EphCal.DotxOrbital * sin(BD2_EphCal.CorrLonAscending);
	//  BD2_EphCal.vz_gk = BD2_EphCal.DotyOrbital * sin(BD2_EphCal.Inc) + BD2_EphCal.yOrbital * BD2_EphCal.DotInc * cos(BD2_EphCal.Inc);
	BD2_EphCal.R_LonAscending[0] = cos(BD2_EphCal.CorrLonAscending);
	BD2_EphCal.R_LonAscending[1] = -sin(BD2_EphCal.CorrLonAscending);
	BD2_EphCal.R_LonAscending[3] = -BD2_EphCal.R_LonAscending[1];
	BD2_EphCal.R_LonAscending[4] = BD2_EphCal.R_LonAscending[0];
	BD2_EphCal.R_LonAscending[8] = 1;
	BD2_EphCal.Rdot_LonAscending[0] = -BD2_EphCal.DotCorrLonAscending * sin(BD2_EphCal.CorrLonAscending);
	BD2_EphCal.Rdot_LonAscending[1] = -BD2_EphCal.DotCorrLonAscending * cos(BD2_EphCal.CorrLonAscending);
	BD2_EphCal.Rdot_LonAscending[3] = -BD2_EphCal.Rdot_LonAscending[1];
	BD2_EphCal.Rdot_LonAscending[4] = BD2_EphCal.Rdot_LonAscending[0];
	BD2_EphCal.R_Inc[0] = 1;
	BD2_EphCal.R_Inc[4] = cos(BD2_EphCal.Inc);
	BD2_EphCal.R_Inc[5] = -sin(BD2_EphCal.Inc);
	BD2_EphCal.R_Inc[7] = -BD2_EphCal.R_Inc[5];
	BD2_EphCal.R_Inc[8] = BD2_EphCal.R_Inc[4];
	BD2_EphCal.Rdot_Inc[4] = -BD2_EphCal.DotInc * sin(BD2_EphCal.Inc);
	BD2_EphCal.Rdot_Inc[5] = -BD2_EphCal.DotInc * cos(BD2_EphCal.Inc);
	BD2_EphCal.Rdot_Inc[7] = -BD2_EphCal.Rdot_Inc[5];
	BD2_EphCal.Rdot_Inc[8] = BD2_EphCal.Rdot_Inc[4];
	BD2_EphCal.r_orb[0] = BD2_EphCal.xOrbital;
	BD2_EphCal.r_orb[1] = BD2_EphCal.yOrbital;
	BD2_EphCal.rdot_orb[0] = BD2_EphCal.DotxOrbital;
	BD2_EphCal.rdot_orb[1] = BD2_EphCal.DotyOrbital;
	MatrixMult(BD2_EphCal.R_ECEF, BD2_EphCal.R_LonAscending, BD2_EphCal.R_Inc, 3, 3, 3, 3);
	MatrixMult(BD2_EphCal.tmp_mat, BD2_EphCal.Rdot_LonAscending, BD2_EphCal.R_Inc, 3, 3, 3, 3);
	MatrixMult(BD2_EphCal.Rdot_ECEF, BD2_EphCal.R_LonAscending, BD2_EphCal.Rdot_Inc, 3, 3, 3, 3);
	MatrixAdd(BD2_EphCal.Rdot_ECEF, BD2_EphCal.Rdot_ECEF, BD2_EphCal.tmp_mat, 3, 3);
	MatrixMult(BD2_EphCal.tmp_mat, BD2_EphCal.Rdot_ECEF, BD2_EphCal.r_orb, 3, 3, 3, 1);
	MatrixMult(BD2_EphCal.vel, BD2_EphCal.R_ECEF, BD2_EphCal.rdot_orb, 3, 3, 3, 1);
	MatrixAdd(BD2_EphCal.vel, BD2_EphCal.vel, BD2_EphCal.tmp_mat, 3, 1);
	if (prnnum <= 5)
	{
		BD2_EphCal.R_LonAscending[0] = cos(BD2_EphCal.TempData);
		BD2_EphCal.R_LonAscending[1] = sin(BD2_EphCal.TempData);
		BD2_EphCal.R_LonAscending[3] = -BD2_EphCal.R_LonAscending[1];
		BD2_EphCal.R_LonAscending[4] = BD2_EphCal.R_LonAscending[0];
		BD2_EphCal.R_LonAscending[8] = 1;
		BD2_EphCal.Rdot_LonAscending[0] = -CGS_ROTATEVEL * sin(BD2_EphCal.TempData);
		BD2_EphCal.Rdot_LonAscending[1] = CGS_ROTATEVEL * cos(BD2_EphCal.TempData);
		BD2_EphCal.Rdot_LonAscending[3] = -BD2_EphCal.Rdot_LonAscending[1];
		BD2_EphCal.Rdot_LonAscending[4] = BD2_EphCal.Rdot_LonAscending[0];
		BD2_EphCal.R_Inc[0] = 1;
		BD2_EphCal.R_Inc[4] = CGS_COS_N5;
		BD2_EphCal.R_Inc[5] = CGS_SIN_N5;
		BD2_EphCal.R_Inc[7] = -CGS_SIN_N5;
		BD2_EphCal.R_Inc[8] = CGS_COS_N5;
		BD2_EphCal.r_orb[0] = BD2_EphCal.x_gk;
		BD2_EphCal.r_orb[1] = BD2_EphCal.y_gk;
		BD2_EphCal.r_orb[2] = BD2_EphCal.z_gk;
		memcpy(BD2_EphCal.rdot_orb, BD2_EphCal.vel, sizeof(double) * 3);
		MatrixMult(BD2_EphCal.R_ECEF, BD2_EphCal.R_LonAscending, BD2_EphCal.R_Inc, 3, 3, 3, 3);
		MatrixMult(BD2_EphCal.Rdot_ECEF, BD2_EphCal.Rdot_LonAscending, BD2_EphCal.R_Inc, 3, 3, 3, 3);
		MatrixMult(BD2_EphCal.tmp_mat, BD2_EphCal.Rdot_ECEF, BD2_EphCal.r_orb, 3, 3, 3, 1);
		MatrixMult(BD2_EphCal.vel, BD2_EphCal.R_ECEF, BD2_EphCal.rdot_orb, 3, 3, 3, 1);
		MatrixAdd(BD2_EphCal.vel, BD2_EphCal.vel, BD2_EphCal.tmp_mat, 3, 1);
		//      memset(BD2_EphCal.vel, 0, sizeof(double)*3);
		//      snprintf(test_str, 512, "sv #%02d, vel %10.6f, %10.6f, %10.6f\r\n", prnnum, BD2_EphCal.vel[0], BD2_EphCal.vel[1], BD2_EphCal.vel[2]);
		//      USR_Ser_WrStr (test_str, BSP_SER_COMM_UART_03);
	}
	databuff_obsbuffdata[prnnum].SVloc.Vx = BD2_EphCal.vel[0];
	databuff_obsbuffdata[prnnum].SVloc.Vy = BD2_EphCal.vel[1];
	databuff_obsbuffdata[prnnum].SVloc.Vz = BD2_EphCal.vel[2];
	//if (prnnum <=2)
	/*  snprintf(test_str, 512, " SV%d %d %10.4f, %10.4f, %10.4f, ",  prnnum,prnnum,databuff_obsbuffdata[prnnum].SVloc.Vx,databuff_obsbuffdata[prnnum].SVloc.Vy,databuff_obsbuffdata[prnnum].SVloc.Vz);
		USR_Ser_WrStr (test_str, BSP_SER_COMM_UART_03);

		snprintf(test_str, 512, "%10.4f, %10.4f, %10.4f, ",databuff_obsbuffdata[prnnum].SVloc.x,databuff_obsbuffdata[prnnum].SVloc.y,databuff_obsbuffdata[prnnum].SVloc.z);
		USR_Ser_WrStr (test_str, BSP_SER_COMM_UART_03);

		snprintf(test_str, 512, "%10.4f\r\n",BD2_EphCal.elapte);
		USR_Ser_WrStr (test_str, BSP_SER_COMM_UART_03);*/
		//}
};